using System;
using System.Collections.Generic;
using System.Text;

namespace Blimpbots.Recognition.Algorithms
{
    public class SimpleAngleAverage
    {
        private double _sSum, _cSum;

        public void Weaken(double factor)
        {
            double len = Math.Sqrt(_sSum*_sSum + _cSum*_cSum);
            _sSum /= (len + factor);
            _cSum /= (len + factor);
        }

        public void Update(double ang)
        {
            _sSum += Math.Sin(ang);
            _cSum += Math.Cos(ang);
        }

        public double Angle()
        {
            return Math.Atan2(_sSum, _cSum);
        }
    }

    public class SimpleAngleMedian
    {
        List<double> angles = new List<double>();

        public void Update(double ang)
        {
            angles.Add(Angles.Normalize(ang));
        }

        public double Angle()
        {
            double bestAng = 0;
            double bestCount = 0;
            foreach(double ang in angles)
            {
                double consistent = 0;
                foreach (double ang2 in angles)
                {
                    consistent += 1.0 / (0.5 + Math.Pow(Angles.Difference(ang, ang2),2));
                }
                if (consistent > bestCount)
                {
                    bestCount = consistent;
                    bestAng = ang;
                }
            }

            return bestAng;
        }
    }



    public class Angles
    {
        public static double Difference(double first, double second)
        {
            double difference = (second - first) % (Math.PI*2);
            if (difference > Math.PI)
                difference -= Math.PI*2;
            if (difference < -Math.PI)
                difference += Math.PI*2;
            return difference;
        }

        public static double Normalize(double ang)
        {
            if (ang > Math.PI * 2)
                ang -= Math.PI * 2;
            if (ang < 0)
                ang += Math.PI * 2;
            return ang;
        }
    }


    public class SimpleAngleFilter
    {
        private double _ang;
        private double _angStd = 100;
        
        //TODO: use angular velocity here!!!
        public double Predict(double t)
        {
            return _ang;
        }

        public SimpleAngleFilter(double ang, double angStd)
        {
            _ang = ang;
            _angStd = angStd;
        }

        public double Update(double estAng, double estStd)
        {
            double offset = Math.PI - _ang;
            estAng = Normalize(estAng + offset);

            double dA = DeltaMagnitude(estAng);
            double wt = 1.0 / (estStd * dA + 0.1);

            _angStd = (_angStd * 5 + dA) / 6;
            double oldWt = 1.0 / _angStd;

            _ang = ((_ang + offset) * oldWt + estAng * wt) / (wt + oldWt);
            _ang -= offset;
            _ang = Normalize(_ang);

            // HACK: if the standard deviation is really high,
            //       just use the current angle as the estimate
            if (_angStd > 3.14) _ang = (estAng - offset);

            return _ang;
        }

        private double DeltaMagnitude(double estAng)
        {
            double offset = Math.PI - _ang;
            estAng = Normalize(estAng + offset);
            return Math.Abs(((_ang + offset) - estAng));
        }

        public double Normalize(double ang)
        {
            if (ang > Math.PI * 2)
                ang -= Math.PI * 2;
            if (ang < 0)
                ang += Math.PI * 2;
            return ang;
        }
    }
}
